#include <ros/ros.h>
#include <stdio.h>
#include <math.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include <Eigen/Core>
#include <gazebo_msgs/ModelStates.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Int8.h>
#include <std_msgs/String.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_srvs/SetBool.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h>
#include <sensor_msgs/NavSatFix.h>
#include <nav_msgs/Odometry.h>
#include <numeric>
#include "tello_baotou/Position.h"

#define PI 3.1415926

using namespace std;
using namespace Eigen;

double t = 0, tstart = 0, T[1000] = {0};
int command = 0, flag = 0;
int track_point_times_ = 1;
double pos_init[3], pos_traget[3];
double r_circle, pi_circle;
double tello_center[3];
int i = 0, j = 0;

typedef struct
{
  double t;
  double yaw;
  Eigen::Vector3d pos;
  string type;
  double omega;
} WAYPOINT;
std::vector<WAYPOINT> traj_points;

void cb_readPos_1(const tello_baotou::Position::ConstPtr &msg)
{
  tello_baotou::Position pos;
  pos = *msg;

  tello_center[0] = pos.position_x;
  tello_center[1] = pos.position_y;
  cout<<"收到中心位置"<<tello_center[0]<<" "<<tello_center[1]<<endl;
}

void cb_command(const std_msgs::Int32::ConstPtr &msg)
{
	std_msgs::Int32 cmd_data;
	cmd_data = *msg;
	command = cmd_data.data;
	cout << "receive command: " << command << endl;
	if (command == 50 && flag == 0)
	{
		tstart = ros::Time::now().toSec();
		flag = 1;
	}
}

int main(int argc, char *argv[])
{
  ros::init(argc, argv, "pos_controller");
  ros::NodeHandle nh("");
  // 定义发布与订阅
  ros::Subscriber cmd_sub1 = nh.subscribe("/command", 10, cb_command);
  ros::Publisher poscmd1 = nh.advertise<tello_baotou::Position>("/pos_cmd", 10);

  ros::Subscriber pos_sub1 = nh.subscribe("/tello_center", 10, cb_readPos_1);
  ros::NodeHandle nh_private("~");//设置私人参数服务器
  XmlRpc::XmlRpcValue ID;
  XmlRpc::XmlRpcValue waypoints;
  nh_private.getParam("ID",ID);
  nh_private.getParam("waypoints", waypoints);
  ROS_ASSERT(waypoints.getType() == XmlRpc::XmlRpcValue::TypeArray);//检测是否读取到正确数据
  tello_baotou::Position hover; // 期望的位置点
  // 录入位置点信息
  for (i = 0; i < waypoints.size(); ++i)
  {
    XmlRpc::XmlRpcValue point = waypoints[i];
    WAYPOINT traj_point;
    traj_point.t = point["t"];
    traj_point.yaw = point["yaw"];
    traj_point.pos[0] = point["pos"][0];
    traj_point.pos[1] = point["pos"][1];
    traj_point.pos[2] = point["pos"][2];
    string type = point["type"];
    traj_point.type = type;
    cout<<traj_point.pos[0]<<" "<<traj_point.pos[1]<<endl;
    if (traj_point.type == "circle_center")
    {
      traj_point.omega = point["omega"];
    }
    traj_points.emplace_back(traj_point);
    cout << "traj_points: " << traj_points[i].pos << endl;
    T[i] = traj_points[i].t;
  }
  ros::Duration(0.5).sleep();//用于确保pos_init可以读取到tello_center的数据
  ros::spinOnce();
  // 重置i的计数,并确定初始位置
  i = 0;
  pos_init[0] = tello_center[0];
  pos_init[1] = tello_center[1];
  ros::Rate loop_rate(100);
  while(ros::ok())
  {
    t = ros::Time::now().toSec() - tstart;
    cout << "t:" << t << endl;
    cout << "T[i]:" << T[i]<<endl;
    if (command == 50) // 开始形成编队
    {
      if (i < waypoints.size())
      {
        if(i>0)
          cout<<"T[i-1]:"<<T[i-1]<<endl;
        cout<<"i:"<<i<<endl;
        pos_traget[0] = traj_points[i].pos[0];
        pos_traget[1] = traj_points[i].pos[1];
        cout<<"初始位置"<<pos_init[0]<<" "<<pos_init[1]<<endl;
        cout<<"期望位置"<<pos_traget[0]<<" "<<pos_traget[1]<<endl;
        cout<<"位置指令"<<hover.position_x<<" "<<hover.position_y<<endl;
        // 根据开始fly_to_car的时间计算本次控制循环的期望位置：hover_x,hover_y,本质为逐步改变期望位置
        if (traj_points[i].type == "fixed")
        {
          hover.position_x = pos_init[0] + (pos_traget[0] - pos_init[0]) * (t - T[i-1]) / (T[i] - T[i-1]);
          hover.position_y = pos_init[1] + (pos_traget[1] - pos_init[1]) * (t - T[i-1]) / (T[i] - T[i-1]);
        }
        if (traj_points[i].type == "circle_center")
        {
          r_circle = sqrt(pow(pos_traget[0] - pos_init[0], 2) + pow(pos_traget[1] - pos_init[1], 2)); // 计算半径
          pi_circle = atan2(pos_traget[1] - pos_init[1], pos_traget[0] - pos_init[0])+PI;
          hover.position_x = pos_traget[0] + r_circle * cos(traj_points[i].omega * (t - T[i-1]) + pi_circle);
          hover.position_y = pos_traget[1] + r_circle * sin(traj_points[i].omega * (t - T[i-1]) + pi_circle);
        }
        poscmd1.publish(hover);
        if (abs(t - T[i]) < 0.05) // 认为到达状态切换位置
        {
          pos_init[0] = tello_center[0];
          pos_init[1] = tello_center[1];
          i++;
        }
      }
      else
      {
        cout<<"飞机完成任务，开始悬停"<<endl;
        hover.position_x = tello_center[0];
        hover.position_y = tello_center[1];
        poscmd1.publish(hover);
      }
    }
    ros::spinOnce();
		loop_rate.sleep();
  }
}